#include <rclcpp/rclcpp.hpp>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include "recognition_interface/srv/get_ped_pos.hpp"
#include "recognition_interface/msg/rectangle.hpp"

class PersonDetectionServer : public rclcpp::Node
{
public:
    PersonDetectionServer() : Node("person_detection_server")
    {
        // 初始化HOG检测器
        hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
        
        // 创建服务
        service_ = create_service<recognition_interface::srv::GetPedPos>(
            "detect_person",
            std::bind(&PersonDetectionServer::detect_callback, this,
                      std::placeholders::_1, std::placeholders::_2));
                      
        RCLCPP_INFO(get_logger(), "Person detection server is ready.");
    }

private:
    void detect_callback(
        const std::shared_ptr<recognition_interface::srv::GetPedPos::Request> request,
        std::shared_ptr<recognition_interface::srv::GetPedPos::Response> response)
    {
        // 将ROS图像消息转换为OpenCV格式
        cv_bridge::CvImagePtr cv_ptr;
        try
        {
            cv_ptr = cv_bridge::toCvCopy(request->imgmsg, sensor_msgs::image_encodings::BGR8);
        }
        catch (cv_bridge::Exception& e)
        {
            RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what());
            return;
        }

        // 检测行人
        std::vector<cv::Rect> detected;
        std::vector<double> weights;
        hog_.detectMultiScale(cv_ptr->image, detected, weights,
                            0.0, // hit threshold
                            cv::Size(8,8), // win stride
                            cv::Size(32,32), // padding
                            1.05, // scale
                            2.0); // final threshold

        response->is_detected = !detected.empty();
        
        // 如果检测到行人，返回检测到的行人的坐标
        if (!detected.empty())
        {
            response->rectangles = std::vector<recognition_interface::msg::Rectangle>(detected.size());
            for(size_t i = 0; i < detected.size(); ++i){
                response->rectangles[i].side = {
                    static_cast<int32_t>(detected[i].x),
                    static_cast<int32_t>(detected[i].y),
                    static_cast<int32_t>(detected[i].width),
                    static_cast<int32_t>(detected[i].height)
                };
            }
        }
    }

    cv::HOGDescriptor hog_;
    rclcpp::Service<recognition_interface::srv::GetPedPos>::SharedPtr service_;
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PersonDetectionServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
